//
// Created by 16933 on 2022/5/2.
//

#include "bsp_Motor.h"
void encoder_data_handle(motor_measure_t *ptr ,uint16_t data)
{
    ptr->temp_sum = 0;
    ptr->last_ecd      = ptr->ecd;
    ptr->ecd = data;
    if (ptr->ecd - ptr->last_ecd > (PULSE_ONE_LAP/2))
    {
        ptr->round_cnt--;
        ptr->ecd_raw_rate = ptr->ecd - ptr->last_ecd - PULSE_ONE_LAP;
    }
    else if (ptr->ecd - ptr->last_ecd < -(PULSE_ONE_LAP/2))
    {
        ptr->round_cnt++;
        ptr->ecd_raw_rate = ptr->ecd - ptr->last_ecd + PULSE_ONE_LAP;
    }
    else
    {
        ptr->ecd_raw_rate = ptr->ecd - ptr->last_ecd;
    }
    ptr->total_ecd_last = ptr->total_ecd;
    ptr->total_ecd = ptr->round_cnt * PULSE_ONE_LAP + ptr->ecd;
    ptr->total_ecd_delta = ptr->total_ecd-ptr->total_ecd_last;
    ptr->total_angle = ptr->total_ecd * 360 / PULSE_ONE_LAP;
    ptr->speed_rpm = (((ptr->ecd_raw_rate/DELAT_T)))*10.0;
    if (ptr->speed_rpm>SPEED_MAX)
    {
        ptr->speed_rpm = SPEED_MAX;
    }
    if(ptr->speed_rpm<-SPEED_MAX)
    {
        ptr->speed_rpm = -SPEED_MAX;
    }

//            (float)(((float)ptr->ecd_raw_rate/(float)(PULSE_ONE_LAP*DELAT_T))*1000*60);
}

/*** 自定义函数 ***/
void encoder_reset(motor_measure_t *ptr)
{
    ptr->ecd = 0;
    ptr->last_ecd = 0;
    ptr->speed_rpm = 0;
    ptr->ecd_raw_rate = 0;
    ptr->round_cnt = 0;
    ptr->total_ecd = 0;
    ptr->total_angle = 0;
    ptr->buf_cut = 0;
    ptr->temp_sum = 0;
}
int encoder_filter(int encoder_left,int encoder_right)
{
    static float Velocity=0,Encoder_Least=0,Encoder=0,Movement=0;
    static float Encoder_Integral=0;
    Encoder_Least = (encoder_left+encoder_right)-0;
    Encoder *= 0.8;
    Encoder += Encoder_Least*0.2;
    return Encoder;
}